import java.awt.Color;
import java.awt.Dimension;
import java.awt.Graphics;
import java.awt.Graphics2D;
import java.awt.GridLayout;
import java.awt.geom.Line2D;
import java.awt.geom.Point2D;
import java.util.ArrayList;

import javax.swing.BorderFactory;
import javax.swing.JPanel;

class Line{
    double start_x=-1;
	double start_y=-1;
	
	double end_x=-1;
	double end_y=-1;
	
	
	
	public Line(){}
	
}

public class RobotPanel extends JPanel{

	ArrayList<Robot> robots;
	ArrayList<Block> blocks;
	ArrayList<Line> lines;
	ArrayList<Cell> cells;
	
	ArrayList<Cell> paths;
	ArrayList<double[]> splines;
	
	//temp for debug
	Cell start_global=null; Cell end_global=null;
	
	double[] c = new double[15];
	double[] spline1; 
    double[] spline2; 
    double[] spline3; 
    
	
    boolean draw_lines;
	
	public RobotPanel(){
		
		this.setLayout(new GridLayout(10,1));
		this.setBorder(BorderFactory.createLineBorder(Color.black));
		this.setSize(new Dimension(500,500));
		this.setPreferredSize(new Dimension(500,500));
		this.setVisible(true);
		
		robots = new ArrayList<Robot>();
		blocks = new ArrayList<Block>();
		lines = new ArrayList<Line>();
		cells = new ArrayList<Cell>();
		splines = new ArrayList<double[]>();
		
		paths = new ArrayList<Cell>();
		
		draw_lines = false;
	}
	
	 public double[] spline (ArrayList<Cell> shortestPath)
	  {    
		//c contains the points which will be interpolated
			c = new double[shortestPath.size()*3];
			int y = 0;
		    for(Cell cell : shortestPath){
			    	c[y] = cell.getX();
			    	c[y+1] = cell.getY();
			    	c[y+2] = 0.0;
			    	y+=3;
			    }
		 
		spline1 = SplineFactory.createBezier (c,     20);//Different Interpolation Techniques
	    spline2 = SplineFactory.createCubic (c,      20);
	   // spline3 = SplineFactory.createCatmullRom (c, 20); 
	    
	     System.out.println ("-- Cubic");
		 for (int i = 0; i < spline2.length; i+=3)
			 System.out.println (spline2[i] + "," + spline2[i+1] + "," + spline2[i+2]);
	    
	    return spline2;
	    
	    //System.out.println ("-- Bezier");
	   // for (int i = 0; i < spline1.length; i+=3)
	      //System.out.println (spline1[i] + "," + spline1[i+1] + "," + spline1[i+2]);
	    
	   // System.out.println ("-- Cubic");
	   // for (int i = 0; i < spline2.length; i+=3)
	      //System.out.println (spline2[i] + "," + spline2[i+1] + "," + spline2[i+2]);

	   // System.out.println ("-- Catmull-Rom");
	   // for (int i = 0; i < spline3.length; i+=3)
	      //System.out.println (spline3[i] + "," + spline3[i+1] + "," + spline3[i+2]);
	  }

	public void paintComponent(Graphics g){
		super.paintComponent(g);
		Graphics2D g2d = (Graphics2D) g;
		
		for(Robot rob: robots){
			//Call Move(lights)
			g2d.setColor(Color.red);
			g2d.fillOval((int)rob.getStartX(), (int)rob.getStartY(), 10, 10);
			g2d.setColor(Color.blue);
			//do not draw endpoint if it has not been set
			if(rob.getEndX()>0 && rob.getEndY()>0)
				g2d.fillOval((int)rob.getEndX(), (int)rob.getEndY(), 10, 10);
		}
		
		g2d.setColor(Color.black);
		for(Block block: blocks){
			g2d.fillRect((int)block.getPosX(), (int)block.getPosY(), block.getSize(), block.getSize());
		}
		
		if(draw_lines)
		{
			g2d.setColor(Color.black);
			for(Line line: lines)
			{
				g2d.drawLine((int)line.start_x, (int)line.start_y, (int)line.end_x, (int)line.end_y);
			}
		}
		
		if(splines!=null && splines.size()!=0){
			System.out.println("Splines is not NULL");
			for(int x = 0; x < splines.size(); x++){
				System.out.println("There is at least 1 Spline");
				for(int y = 0; y < splines.get(x).length - 3; y+=3){
					System.out.println("Drawing a line in the Spline");
					g2d.drawLine((int)splines.get(x)[y], (int)splines.get(x)[y+1], (int)splines.get(x)[y+3], (int)splines.get(x)[y+4]);
				}
			}
		}
		
		
		//Debugging: Draws lines between adjacent cells
		/*g2d.setColor(Color.green);
		for(Cell cell: cells)
		{
			for(Cell neighbor: cell.getAdjacent())
			{
				g2d.drawLine((int)cell.getX(), (int)cell.getY(), (int)neighbor.getX(), (int)neighbor.getY());
			}
		}
		g2d.setColor(Color.cyan);
		if(start_global!=null)
			for(Cell cell:start_global.getAdjacent())
			{
				g2d.drawLine((int)start_global.getX(), (int)start_global.getY(), (int)cell.getX(), (int)cell.getY());
			}*/

}
	
	public void addRobot(Robot rob)
	{
		robots.add(rob);
	}
	
	public void addBlock(Block block)
	{
		boolean add = true;
		for(int x = 0; x < blocks.size(); x++){		
			if(block.getPosX()>= blocks.get(x).getPosX() - block.getSize() && block.getPosX()<= blocks.get(x).getPosX() + blocks.get(x).getSize()){
				if(block.getPosY()>= blocks.get(x).getPosY() - block.getSize() && block.getPosY() <= blocks.get(x).getPosY() + blocks.get(x).getSize()){
					add = false;
					System.out.println("Block cannot be drawn here!");
					x = blocks.size()+1;
					return;
				}
			}
		}
		if(add){
			blocks.add(block);
		}
		lines.clear();
	}
	
	public Line upLine(double x, double y, Block block)
	{
		Line new_line = new Line();
		new_line.start_x = x;
		new_line.end_x = x;
		new_line.start_y = 0;
		new_line.end_y = y;
		
		//Check intersection with other blocks
		for(Block test_block:blocks)
		{
			//Does not count if it is the same block
			if(!block.equals(test_block))
			{
				if(x>=test_block.getPosX()&& x<=test_block.getPosX()+test_block.getSize() && test_block.getPosY()+test_block.getSize()<y)
				{
					if(test_block.getPosY()+test_block.getSize()>new_line.start_y)
					{
						new_line.start_y = test_block.getPosY()+test_block.getSize();
					}
				}
			}
		}
		
		//Order the start_y and end_y from smaller to larger
		if(new_line.end_y<new_line.start_y)
		{
			double temp = new_line.start_y;
			new_line.start_y = new_line.end_y;
			new_line.end_y = temp;
		}
		
		return new_line;
		
	}
	
	public Line downLine(double x, double y, Block block)
	{
		Line new_line = new Line();
		new_line.start_x = x;
		new_line.end_x = x;
		new_line.start_y = y;
		new_line.end_y = 500;
		
		for(Block test_block:blocks)
		{
		//Does not count if it is the same block
			if(!block.equals(test_block))
			{
				if(x>=test_block.getPosX()&& x<=test_block.getPosX()+test_block.getSize() && test_block.getPosY()>y)
				{
					if(test_block.getPosY()<new_line.end_y)
					{
						new_line.end_y = test_block.getPosY();
					}
				}
			}
		}
		
		//Order the start_y and end_y from smaller to larger
		if(new_line.end_y<new_line.start_y)
		{
			double temp = new_line.start_y;
			new_line.start_y = new_line.end_y;
			new_line.end_y = temp;
		}
			
		
		return new_line;
	}
	
	public void compute_lines()
	{
		for(Block block:blocks)
		{
			//draw a line straight up and straight down from each point until it intersects geometry(other blocks or screen edge)
			Line topLeft = upLine(block.getPosX(),block.getPosY(),block);
			Line bottomLeft = downLine(block.getPosX(),block.getPosY()+block.getSize(),block);
			Line topRight = upLine(block.getPosX()+block.getSize(),block.getPosY(),block);
			Line bottomRight = downLine(block.getPosX()+block.getSize(), block.getPosY()+block.getSize(),block);
			
			//combine lines if they are the same???
			lines.add(topLeft);
			lines.add(bottomLeft);
			lines.add(topRight);
			lines.add(bottomRight);
		}
	}
	
	public boolean lines_intersect(double x1, double y1,double x2, double y2, double x3, double y3, double x4, double y4)
	{
		//collinear lines seem to be a problem so we will do a separate check
		if((x1==x2 && x2==x3 && x3==x4) || (y1==y2 && y2==y3 && y3==y4))
			return true;
		
		double ua = ((x4-x3)*(y1-y3)-(y4-y3)*(x1-x3))/((y4-y3)*(x2-x1)-(x4-x3)*(y2-y1));
		double ub = ((x2-x1)*(y1-y3)-(y2-y1)*(x1-x3))/((y4-y3)*(x2-x1)-(x4-x3)*(y2-y1));
		
		if((ua >0 && ua<1) && (ub>0 && ub<1))
			return true;
		else
			return false;
	}
	
	public void create_cells()
	{
		//sort the lines from smallest x value to largest x value
		//Find adjacent lines
		cells.clear();
		
		//sort according to x value
		for(int i = 0; i<lines.size()-1; i++)
		{
			for(int j=i+1; j<lines.size(); j++)
			{
				Line line1 = lines.get(i);
				Line line2 = lines.get(j);
				if(line2.start_x < line1.start_x)
				{
					//Line temp = lines.get(i);
					lines.set(i,line2);
					lines.set(j,line1);
				}
			}
			
		}
		
		//Create Cells
		for(Line line:lines)
		{
			cells.add(new Cell(line));
		}
		
		//Determine Adjacency
		for(Cell cell: cells)
		{
			
			for(Cell cell2: cells)
			{
				if(!cell.equals(cell2) && cell.getX()<cell2.getX()){
					if(!cell.getAdjacent().isEmpty() && cell2.getX() > cell.getAdjacent().get(0).getX())
						break;
					boolean collision = false;
					for(Block block: blocks){
					 if(lines_intersect(cell.getX(),cell.getY(),cell2.getX(),cell2.getY(),block.getPosX(),block.getPosY(),block.getPosX()+block.getSize(),block.getPosY())
							|| lines_intersect(cell.getX(),cell.getY(),cell2.getX(),cell2.getY(),block.getPosX(),block.getPosY(),block.getPosX(),block.getPosY()+block.getSize())
							|| lines_intersect(cell.getX(),cell.getY(),cell2.getX(),cell2.getY(),block.getPosX()+block.getSize(),block.getPosY(),block.getPosX()+block.getSize(),block.getPosY()+block.getSize())
							|| lines_intersect(cell.getX(),cell.getY(),cell2.getX(),cell2.getY(),block.getPosX(),block.getPosY()+block.getSize(),block.getPosX()+block.getSize(),block.getPosY()+block.getSize()))
							{
								collision = true;
								break;
							}
					}
					if(!collision)cell.addAdj(cell2);
					
				}
				
			}
		}
	}
	
	//Assuming vertical lines
	public double distance(Cell L1, Cell L2)
	{
		double dist = Math.sqrt((Math.pow(L1.getX()- L2.getX(),2) + Math.pow(L1.getY()-L2.getY(),2)));
		return dist;
	}
	
	public Cell Dijkstra(Robot robot)
	{
		ArrayList<Cell> shortestPath = new ArrayList<Cell>();
		ArrayList<Cell> cells_copy = new ArrayList<Cell>();
		
		//Copy the cells list
		for(int i = 0; i<cells.size(); i++)
		{
			Cell cell = cells.get(i);
			 cells_copy.add(cell);
		}
		
		//Create a "cell" for the start point, should have a path of 0
		Cell start = new Cell(robot.getStartX(),robot.getStartY(),0);
		start_global = start;
		//adjacency: find those lines that have closest x that is greater ... y constraints???
		for(Cell cell: cells_copy){
			if(!start.getAdjacent().isEmpty() && cell.getX() > start.getAdjacent().get(0).getX())
				break;
			else
			{
				if(cell.getX() > start.getX() && cell.line.start_y < start.getY())
					start.addAdj(cell);
			}
		}
		cells_copy.add(start);
		
		//Create a cell for the end point
		Cell end = new Cell(robot.getEndX(),robot.getEndY(),100000000);
		end_global=end;
		//Adjacency is a little harder ... add this to the adjacency list of other cells...
		Cell found = null;
		for(int i=cells.size()-1; i>-1; i--)
		{
			Cell cell = cells.get(i);
			if(cell.getX()<end.getX()){
				if(found!=null  && cell.getX()< found.getX())
					break;
				boolean collision = false;
				for(Block block: blocks)
				{
					if(lines_intersect(end.getX(),end.getY(),cell.getX(),cell.getY(),block.getPosX(),block.getPosY(),block.getPosX()+block.getSize(),block.getPosY())
					|| lines_intersect(end.getX(),end.getY(),cell.getX(),cell.getY(),block.getPosX(),block.getPosY(),block.getPosX(),block.getPosY()+block.getSize())
					|| lines_intersect(end.getX(),end.getY(),cell.getX(),cell.getY(),block.getPosX()+block.getSize(),block.getPosY(),block.getPosX()+block.getSize(),block.getPosY()+block.getSize())
					|| lines_intersect(end.getX(),end.getY(),cell.getX(),cell.getY(),block.getPosX(),block.getPosY()+block.getSize(),block.getPosX()+block.getSize(),block.getPosY()+block.getSize()))
					{
						collision = true;
					}
				}
				//if(cell.getX()<end.getX() && cell.line.end_y > end.getY())
				if(!collision)
				{
					cells_copy.get(i).addAdj(end);
					found = cell;
				}
			}
		}
		cells_copy.add(end);
		
		while(!cells_copy.isEmpty()){
			//Find minimum
			Cell min = null;
			for(Cell line: cells_copy)
			{
				if(min == null)
					min=line;
				else
					if(line.path<min.path)
						min=line;
			}
			cells_copy.remove(min);
			
			//Add this cell to shortest path
			shortestPath.add(min);
			
			//Relax the adjacent edges for this line
			for(Cell adj: min.getAdjacent())
			{
				if(adj.path > min.path + distance(adj,min))
				{
					adj.path = min.path + distance(adj,min);
					adj.parent = min;
				}
			}
		}
		
		if(end.parent == null)
			end.parent = start;
		//return shortestPath;
		return end;
	}
	
	public void CreatePaths()
	{
		splines.clear();
		paths.clear();
		create_cells();
		splines = new ArrayList<double[]>();
		//System.out.println("Create Paths called: Size of Robots: " + robots.size());
		for(Robot robot: robots)
		{
			if(robot.getEndX()>0 && robot.getEndY()>0 && robot.getStartX()>0 && robot.getEndY()>0){
				Cell shortestPath = Dijkstra(robot);
				//ArrayList<Cell> shortestPath = Djikstra(robot);
				//Use these cells to draw interpolated shortest path
				paths.add(shortestPath);
				
				ArrayList<Cell> full_path = new ArrayList<Cell>();
				
				Cell parent = shortestPath;
				while(parent!=null)
				{
					full_path.add(parent);
					parent = parent.parent;
				}
				if(!full_path.isEmpty())
					splines.add(spline(full_path));
			}
			
		}

	}
	
	public void clear()
	{
		robots.clear();
		blocks.clear();
		lines.clear();
		paths.clear();
		cells.clear();
		splines.clear();
		
		repaint();
	}
}
